
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mb_motors_multicopter.c
  * @author     baiyang
  * @date       2023-1-30
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "mb_motors_multicopter.h"

#include <parameter/param.h>
#include <common/time/gp_time.h>
#include <vehicle/vehicle_type.h>
#include <srv_channel/srv_channel.h>
#include <battery_monitor/battery_monitor.h>
/*-----------------------------------macro------------------------------------*/
#if MB_BUILD_TYPE(MICROBEE_BUILD_Plane)
    #define MB_MOTORS_PARAM_PREFIX "Q_M_"
#else
    #define MB_MOTORS_PARAM_PREFIX "MOT_"
#endif
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void output(mb_motors_t motors);
static void output_min(mb_motors_t motors);
static float get_throttle_hover(mb_motors_const_t motors);
static bool is_motor_enabled(mb_motors_t motors, uint8_t i);
static void Log_Write(mb_motors_t motors);
static bool arming_checks(mb_motors_const_t motors, size_t buflen, char *buffer);
static void update_throttle_filter(mb_motors_t motors);
static void save_params_on_disarm(mb_motors_t motors);
static void assign_param(mb_motors_t motors);
/*----------------------------------variable----------------------------------*/
static param_t var_info[] = {
    PARAM_DEFINE_INT16(MOT_YAW_HEADROOM, MB_MOTORS_YAW_HEADROOM_DEFAULT),
    PARAM_DEFINE_FLOAT(MOT_THST_EXPO,    MB_MOTORS_THST_EXPO_DEFAULT),
    PARAM_DEFINE_FLOAT(MOT_SPIN_MAX,     MB_MOTORS_SPIN_MAX_DEFAULT),
    PARAM_DEFINE_FLOAT(MOT_BAT_VOLT_MAX, MB_MOTORS_BAT_VOLT_MAX_DEFAULT),
    PARAM_DEFINE_FLOAT(MOT_BAT_VOLT_MIN, MB_MOTORS_BAT_VOLT_MIN_DEFAULT),
    PARAM_DEFINE_FLOAT(MOT_BAT_CURR_MAX, MB_MOTORS_BAT_CURR_MAX_DEFAULT),
    PARAM_DEFINE_INT8(MOT_PWM_TYPE,      MB_MOTOR_PWM_TYPE_NORMAL),
    PARAM_DEFINE_INT16(MOT_PWM_MIN,      1000),
    PARAM_DEFINE_INT16(MOT_PWM_MAX,      2000),
    PARAM_DEFINE_FLOAT(MOT_SPIN_MIN,     MB_MOTORS_SPIN_MIN_DEFAULT),
    PARAM_DEFINE_FLOAT(MOT_SPIN_ARM,     MB_MOTORS_SPIN_ARM_DEFAULT),
    PARAM_DEFINE_FLOAT(MOT_BAT_CURR_TC,  MB_MOTORS_BAT_CURR_TC_DEFAULT),
    PARAM_DEFINE_FLOAT(MOT_THST_HOVER,   MB_MOTORS_THST_HOVER_DEFAULT),
    PARAM_DEFINE_INT8(MOT_HOVER_LEARN,   MB_MOTOR_HOVER_LEARN_AND_SAVE),
    PARAM_DEFINE_INT8(MOT_SAFE_DISARM,   0),
    PARAM_DEFINE_FLOAT(MOT_YAW_SV_ANGLE, 30.0f),
    PARAM_DEFINE_FLOAT(MOT_SPOOL_TIME,   MB_MOTORS_SPOOL_UP_TIME_DEFAULT),
    PARAM_DEFINE_FLOAT(MOT_BOOST_SCALE,  0.0f),
    PARAM_DEFINE_INT8(MOT_BAT_IDX,       0),
    PARAM_DEFINE_FLOAT(MOT_SLEW_UP_TIME, MB_MOTORS_SLEW_TIME_DEFAULT),
    PARAM_DEFINE_FLOAT(MOT_SLEW_DN_TIME, MB_MOTORS_SLEW_TIME_DEFAULT),
    PARAM_DEFINE_FLOAT(MOT_SAFE_TIME,    MB_MOTORS_SAFE_TIME_DEFAULT),
};
PARAM_GROUP_DEFINE(MOTORS, var_info);

static struct mb_motors_ops motors_ops = {.arming_checks = arming_checks,
                                          .set_roll_pitch = NULL,
                                          .get_throttle_hover = get_throttle_hover,
                                          .get_lost_motor = NULL,
                                          .set_update_rate = NULL,
                                          .init = NULL,
                                          .set_frame_class_and_type = NULL,
                                          .output = output,
                                          .output_min = output_min,
                                          .get_motor_mask = mb_motors_GetMotorMask,
                                          .get_roll_factor = NULL,
                                          .get_pitch_factor = NULL,
                                          .is_motor_enabled = is_motor_enabled,
                                          .init_targets_on_arming = NULL,
                                          .rc_write = NULL,
                                          .Log_Write = Log_Write,
                                          .output_armed_stabilizing = NULL,
                                          .rc_write_angle = NULL,
                                          .rc_set_freq = NULL,
                                          .update_throttle_filter = update_throttle_filter,
                                          .save_params_on_disarm = save_params_on_disarm,
                                          ._get_frame_string = NULL,
                                          .get_type_string = NULL,
                                          ._output_test_seq = NULL,
                                          .assign_param     = assign_param};

static struct mb_motors_multicopter_ops multicopter_ops;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void mb_motors_multicopter_ctor(mb_motors_mc_t motors_mc, mb_motors_mc_ops_t ops_mc, mb_motors_ops_t ops, uint16_t speed_hz)
{
    if (ops_mc == NULL) {
        ops_mc = &multicopter_ops;
    }

    if (ops == NULL) {
        ops = &motors_ops;
    }

    mb_motors_bind_motors_function(ops, &motors_ops);

    mb_motors_ctor(&motors_mc->motors, ops, speed_hz);

    motors_mc->ops_mc = ops_mc;

    motors_mc->_lift_max = 1.0f;
    motors_mc->_throttle_limit = 1.0f;

    lpf_set_cutoff1(&motors_mc->_batt_voltage_filt, MB_MOTORS_BATT_VOLT_FILT_HZ);
    lpf_reset(&motors_mc->_batt_voltage_filt, 1.0f);

    mb_motors_assign_param(&motors_mc->motors);
}

// update_throttle_range - update throttle endpoints
void mb_motors_update_throttle_range(mb_motors_t motors)
{
    mb_motors_mc_t motors_mc = (mb_motors_mc_t)motors;

    // if all outputs are digital adjust the range. We also do this for type PWM_RANGE, as those use the
    // scaled output, which is then mapped to PWM via the SRV_Channel library
    if (srv_channels_have_digital_outputs(mb_motors_get_motor_mask(motors)) || (motors->_pwm_type == MB_MOTOR_PWM_TYPE_PWM_RANGE)) {
        motors_mc->_pwm_min = 1000;
        motors_mc->_pwm_max = 2000;
    }

    srv_hal_set_esc_scaling(mb_motors_get_pwm_output_min(motors), mb_motors_get_pwm_output_max(motors));
}

// update the throttle input filter.  should be called at 100hz
void mb_motors_update_throttle_hover(mb_motors_t motors, float dt)
{
    mb_motors_mc_t motors_mc = (mb_motors_mc_t)motors;

    if (motors_mc->_throttle_hover_learn != MB_MOTOR_HOVER_LEARN_DISABLED) {
        // we have chosen to constrain the hover throttle to be within the range reachable by the third order expo polynomial.
        motors_mc->_throttle_hover = math_constrain_float(motors_mc->_throttle_hover + (dt / (dt + MB_MOTORS_THST_HOVER_TC)) * (mb_motors_get_throttle(motors) - motors_mc->_throttle_hover), MB_MOTORS_THST_HOVER_MIN, MB_MOTORS_THST_HOVER_MAX);
    }
}

// passes throttle directly to all motors for ESC calibration.
//   throttle_input is in the range of 0 ~ 1 where 0 will send get_pwm_output_min() and 1 will send get_pwm_output_max()
void mb_motors_set_throttle_passthrough_for_esc_calibration(mb_motors_t motors, float throttle_input)
{
    mb_motors_mc_t motors_mc = (mb_motors_mc_t)motors;

    if (mb_motors_armed(motors)) {
        uint16_t pwm_out = mb_motors_get_pwm_output_min(motors) + math_constrain_float(throttle_input, 0.0f, 1.0f) * (mb_motors_get_pwm_output_max(motors) - mb_motors_get_pwm_output_min(motors));
        // send the pilot's input directly to each enabled motor
        for (uint16_t i = 0; i < MB_MOTORS_MAX_NUM_MOTORS; i++) {
            if (motors_mc->motor_enabled[i]) {
                mb_motors_rc_write(motors, i, pwm_out);
            }
        }
        // send pwm output to channels used by bicopter
        srv_channels_set_output_pwm(k_throttleRight, pwm_out);
        srv_channels_set_output_pwm(k_throttleLeft, pwm_out);
    }
}

// parameter check for MOT_PWM_MIN/MAX, returns true if parameters are valid
bool mb_motors_check_mot_pwm_params(mb_motors_const_t motors)
{
    mb_motors_mc_t motors_mc = (mb_motors_mc_t)motors;

    // _pwm_min is a value greater than or equal to 1.
    // _pwm_max is greater than _pwm_min.
    // The values of _pwm_min and _pwm_max are positive values.
    if (motors_mc->_pwm_min < 1 || motors_mc->_pwm_min >= motors_mc->_pwm_max) {
        return false;
    }
    return true;
}

// converts desired thrust to linearized actuator output in a range of 0~1
float mb_motors_thrust_to_actuator(mb_motors_const_t motors, float thrust_in)
{
    mb_motors_mc_const_t motors_mc = (mb_motors_mc_const_t)motors;

    thrust_in = math_constrain_float(thrust_in, 0.0f, 1.0f);
    return motors_mc->_spin_min + (motors_mc->_spin_max - motors_mc->_spin_min) * mb_motors_apply_thrust_curve_and_volt_scaling(motors, thrust_in);
}

// inverse of above, tested with AP_Motors/examples/expo_inverse_test
// used to calculate equivelent motor throttle level to direct ouput, used in tailsitter transtions
float mb_motors_actuator_to_thrust(mb_motors_const_t motors, float actuator)
{
    mb_motors_mc_const_t motors_mc = (mb_motors_mc_const_t)motors;

    actuator = (actuator - motors_mc->_spin_min) /  (motors_mc->_spin_max - motors_mc->_spin_min);
    return math_constrain_float(mb_motors_remove_thrust_curve_and_volt_scaling(motors, actuator), 0.0f, 1.0f);
}

// apply_thrust_curve_and_volt_scaling - returns throttle in the range 0 ~ 1
float mb_motors_apply_thrust_curve_and_volt_scaling(mb_motors_const_t motors, float thrust)
{
    mb_motors_mc_const_t motors_mc = (mb_motors_mc_const_t)motors;

    float battery_scale = 1.0f;
    if (math_flt_positive(lpf_get_output(&motors_mc->_batt_voltage_filt))) {
        battery_scale = 1.0f / lpf_get_output(&motors_mc->_batt_voltage_filt);
    }
    // apply thrust curve - domain -1.0 to 1.0, range -1.0 to 1.0
    float thrust_curve_expo = math_constrain_float(motors_mc->_thrust_curve_expo, -1.0f, 1.0f);
    if (math_flt_zero(thrust_curve_expo)) {
        // zero expo means linear, avoid floating point exception for small values
        return motors_mc->_lift_max * thrust * battery_scale;
    }
    float throttle_ratio = ((thrust_curve_expo - 1.0f) + math_sqrtf((1.0f - thrust_curve_expo) * (1.0f - thrust_curve_expo) + 4.0f * thrust_curve_expo * motors_mc->_lift_max * thrust)) / (2.0f * thrust_curve_expo);
    return math_constrain_float(throttle_ratio * battery_scale, 0.0f, 1.0f);
}

// inverse of above, tested with AP_Motors/examples/expo_inverse_test
// used to calculate equivelent motor throttle level to direct ouput, used in tailsitter transtions
float mb_motors_remove_thrust_curve_and_volt_scaling(mb_motors_const_t motors, float throttle)
{
    mb_motors_mc_const_t motors_mc = (mb_motors_mc_const_t)motors;

    float battery_scale = 1.0f;
    if (math_flt_positive(lpf_get_output(&motors_mc->_batt_voltage_filt))) {
        battery_scale = 1.0f / lpf_get_output(&motors_mc->_batt_voltage_filt);
    }
    // apply thrust curve - domain -1.0 to 1.0, range -1.0 to 1.0
    float thrust_curve_expo = math_constrain_float(motors_mc->_thrust_curve_expo, -1.0f, 1.0f);
    if (math_flt_zero(thrust_curve_expo)) {
        // zero expo means linear, avoid floating point exception for small values
        return  throttle / (motors_mc->_lift_max * battery_scale);
    }
    float thrust = ((throttle / battery_scale) * (2.0f * thrust_curve_expo)) - (thrust_curve_expo - 1.0f);
    thrust = (thrust * thrust) - ((1.0f - thrust_curve_expo) * (1.0f - thrust_curve_expo));
    thrust /=  4.0f * thrust_curve_expo * motors_mc->_lift_max;
    return math_constrain_float(thrust, 0.0f, 1.0f);
}

// convert values to PWM min and max if not configured
void mb_motors_convert_pwm_min_max_param(mb_motors_t motors, int16_t radio_min, int16_t radio_max)
{
    mb_motors_mc_t motors_mc = (mb_motors_mc_t)motors;

    const char* group_name = "MOTORS";
    param_t* pwm_min = param_get_by_variable2(group_name, &motors_mc->_pwm_min);
    param_t* pwm_max = param_get_by_variable2(group_name, &motors_mc->_pwm_max);

    if (pwm_min != NULL || pwm_max != NULL) {
        return;
    }

    motors_mc->_pwm_min = radio_min;
    motors_mc->_pwm_max = radio_max;
}

// run spool logic
void mb_motors_output_logic(mb_motors_t motors)
{
    mb_motors_mc_t motors_mc = (mb_motors_mc_t)motors;

    if (mb_motors_armed(motors)) {
        if (motors_mc->_disarm_disable_pwm && (motors_mc->_disarm_safe_timer < motors_mc->_safe_time)) {
            motors_mc->_disarm_safe_timer += motors->_dt;
        } else {
            motors_mc->_disarm_safe_timer = motors_mc->_safe_time;
        }
    } else {
           motors_mc->_disarm_safe_timer = 0.0f;
    }

    // force desired and current spool mode if disarmed or not interlocked
    if (!mb_motors_armed(motors) || !mb_motors_get_interlock(motors)) {
        motors->_spool_desired = MB_MOTOR_DESIRED_SHUT_DOWN;
        motors->_spool_state = MB_MOTOR_SHUT_DOWN;
    }

    if (motors_mc->_spool_up_time < 0.05f) {
        // prevent float exception
        motors_mc->_spool_up_time = 0.05f;
    }

    const float spool_step = motors->_dt / motors_mc->_spool_up_time;
    switch (motors->_spool_state) {
    case MB_MOTOR_SHUT_DOWN:
        // Motors should be stationary.
        // Servos set to their trim values or in a test condition.

        // set limits flags
        motors->limit.roll = true;
        motors->limit.pitch = true;
        motors->limit.yaw = true;
        motors->limit.throttle_lower = true;
        motors->limit.throttle_upper = true;

        // make sure the motors are spooling in the correct direction
        if (motors->_spool_desired != MB_MOTOR_DESIRED_SHUT_DOWN && motors_mc->_disarm_safe_timer >= motors_mc->_safe_time) {
            motors->_spool_state = MB_MOTOR_GROUND_IDLE;
            break;
        }

        // set and increment ramp variables
        motors_mc->_spin_up_ratio = 0.0f;
        motors_mc->_throttle_thrust_max = 0.0f;

        // initialise motor failure variables
        motors->_thrust_boost = false;
        motors->_thrust_boost_ratio = 0.0f;
        break;

    case MB_MOTOR_GROUND_IDLE: {
        // Motors should be stationary or at ground idle.
        // Servos should be moving to correct the current attitude.

        // set limits flags
        motors->limit.roll = true;
        motors->limit.pitch = true;
        motors->limit.yaw = true;
        motors->limit.throttle_lower = true;
        motors->limit.throttle_upper = true;

        // set and increment ramp variables
        switch (motors->_spool_desired) {
        case MB_MOTOR_DESIRED_SHUT_DOWN:
            motors_mc->_spin_up_ratio -= spool_step;
            // constrain ramp value and update mode
            if (motors_mc->_spin_up_ratio <= 0.0f) {
                motors_mc->_spin_up_ratio = 0.0f;
                motors->_spool_state = MB_MOTOR_SHUT_DOWN;
            }
            break;

        case MB_MOTOR_DESIRED_THROTTLE_UNLIMITED:
            motors_mc->_spin_up_ratio += spool_step;
            // constrain ramp value and update mode
            if (motors_mc->_spin_up_ratio >= 1.0f) {
                motors_mc->_spin_up_ratio = 1.0f;
                if (!mb_motors_get_spoolup_block(motors)) {
                    // Only advance from ground idle if spoolup checks have passed
                    motors->_spool_state = MB_MOTOR_SPOOLING_UP;
                }
            }
            break;

        case MB_MOTOR_DESIRED_GROUND_IDLE: {
                float spin_up_armed_ratio = 0.0f;
                if (motors_mc->_spin_min > 0.0f) {
                    spin_up_armed_ratio = motors_mc->_spin_arm / motors_mc->_spin_min;
                }
                motors_mc->_spin_up_ratio += math_constrain_float(spin_up_armed_ratio - motors_mc->_spin_up_ratio, -spool_step, spool_step);
            }
            break;
        }
        motors_mc->_throttle_thrust_max = 0.0f;

        // initialise motor failure variables
        motors->_thrust_boost = false;
        motors->_thrust_boost_ratio = 0.0f;
        break;
    }
    case MB_MOTOR_SPOOLING_UP:
        // Maximum throttle should move from minimum to maximum.
        // Servos should exhibit normal flight behavior.

        // initialize limits flags
        motors->limit.roll = false;
        motors->limit.pitch = false;
        motors->limit.yaw = false;
        motors->limit.throttle_lower = false;
        motors->limit.throttle_upper = false;

        // make sure the motors are spooling in the correct direction
        if (motors->_spool_desired != MB_MOTOR_DESIRED_THROTTLE_UNLIMITED) {
            motors->_spool_state = MB_MOTOR_SPOOLING_DOWN;
            break;
        }

        // set and increment ramp variables
        motors_mc->_spin_up_ratio = 1.0f;
        motors_mc->_throttle_thrust_max += spool_step;

        // constrain ramp value and update mode
        if (motors_mc->_throttle_thrust_max >= MIN(mb_motors_get_throttle(motors), mb_motors_get_current_limit_max_throttle(motors))) {
            motors_mc->_throttle_thrust_max = mb_motors_get_current_limit_max_throttle(motors);
            motors->_spool_state = MB_MOTOR_THROTTLE_UNLIMITED;
        } else if (motors_mc->_throttle_thrust_max < 0.0f) {
            motors_mc->_throttle_thrust_max = 0.0f;
        }

        // initialise motor failure variables
        motors->_thrust_boost = false;
        motors->_thrust_boost_ratio = MAX(0.0f, motors->_thrust_boost_ratio - spool_step);
        break;

    case MB_MOTOR_THROTTLE_UNLIMITED:
        // Throttle should exhibit normal flight behavior.
        // Servos should exhibit normal flight behavior.

        // initialize limits flags
        motors->limit.roll = false;
        motors->limit.pitch = false;
        motors->limit.yaw = false;
        motors->limit.throttle_lower = false;
        motors->limit.throttle_upper = false;

        // make sure the motors are spooling in the correct direction
        if (motors->_spool_desired != MB_MOTOR_DESIRED_THROTTLE_UNLIMITED) {
            motors->_spool_state = MB_MOTOR_SPOOLING_DOWN;
            break;
        }

        // set and increment ramp variables
        motors_mc->_spin_up_ratio = 1.0f;
        motors_mc->_throttle_thrust_max = mb_motors_get_current_limit_max_throttle(motors);

        if (motors->_thrust_boost && !motors->_thrust_balanced) {
            motors->_thrust_boost_ratio = MIN(1.0f, motors->_thrust_boost_ratio + spool_step);
        } else {
            motors->_thrust_boost_ratio = MAX(0.0f, motors->_thrust_boost_ratio - spool_step);
        }
        break;

    case MB_MOTOR_SPOOLING_DOWN:
        // Maximum throttle should move from maximum to minimum.
        // Servos should exhibit normal flight behavior.

        // initialize limits flags
        motors->limit.roll = false;
        motors->limit.pitch = false;
        motors->limit.yaw = false;
        motors->limit.throttle_lower = false;
        motors->limit.throttle_upper = false;

        // make sure the motors are spooling in the correct direction
        if (motors->_spool_desired == MB_MOTOR_DESIRED_THROTTLE_UNLIMITED) {
            motors->_spool_state = MB_MOTOR_SPOOLING_UP;
            break;
        }

        // set and increment ramp variables
        motors_mc->_spin_up_ratio = 1.0f;
        motors_mc->_throttle_thrust_max -= spool_step;

        // constrain ramp value and update mode
        if (motors_mc->_throttle_thrust_max <= 0.0f) {
            motors_mc->_throttle_thrust_max = 0.0f;
        }
        if (motors_mc->_throttle_thrust_max >= mb_motors_get_current_limit_max_throttle(motors)) {
            motors_mc->_throttle_thrust_max = mb_motors_get_current_limit_max_throttle(motors);
        } else if (math_flt_zero(motors_mc->_throttle_thrust_max)) {
            motors->_spool_state = MB_MOTOR_GROUND_IDLE;
        }

        motors->_thrust_boost_ratio = MAX(0.0f, motors->_thrust_boost_ratio - spool_step);
        break;
    }
}

// update_lift_max from battery voltage - used for voltage compensation
void mb_motors_update_lift_max_from_batt_voltage(mb_motors_t motors)
{
    mb_motors_mc_t motors_mc = (mb_motors_mc_t)motors;

    // sanity check battery_voltage_min is not too small
    // if disabled or misconfigured exit immediately
    float _batt_voltage_resting_estimate = battmonitor_voltage_resting_estimate(motors_mc->_batt_idx);
    if ((motors_mc->_batt_voltage_max <= 0) || (motors_mc->_batt_voltage_min >= motors_mc->_batt_voltage_max) || (_batt_voltage_resting_estimate < 0.25f * motors_mc->_batt_voltage_min)) {
        lpf_reset(&motors_mc->_batt_voltage_filt, 1.0f);
        motors_mc->_lift_max = 1.0f;
        return;
    }

    motors_mc->_batt_voltage_min = MAX(motors_mc->_batt_voltage_min, motors_mc->_batt_voltage_max * 0.6f);

    // contrain resting voltage estimate (resting voltage is actual voltage with sag removed based on current draw and resistance)
    _batt_voltage_resting_estimate = math_constrain_float(_batt_voltage_resting_estimate, motors_mc->_batt_voltage_min, motors_mc->_batt_voltage_max);

    // filter at 0.5 Hz
    float batt_voltage_filt = lpf_apply2(&motors_mc->_batt_voltage_filt, _batt_voltage_resting_estimate / motors_mc->_batt_voltage_max, motors->_dt);

    // calculate lift max
    float thrust_curve_expo = math_constrain_float(motors_mc->_thrust_curve_expo, -1.0f, 1.0f);
    motors_mc->_lift_max = batt_voltage_filt * (1.0f - thrust_curve_expo) + thrust_curve_expo * batt_voltage_filt * batt_voltage_filt;
}

// return gain scheduling gain based on voltage and air density
float mb_motors_get_compensation_gain(mb_motors_const_t motors)
{
    mb_motors_mc_t motors_mc = (mb_motors_mc_t)motors;

    // avoid divide by zero
    if (motors_mc->_lift_max <= 0.0f) {
        return 1.0f;
    }

    float ret = 1.0f / motors_mc->_lift_max;

#if MB_MOTORS_DENSITY_COMP == 1
    // air density ratio is increasing in density / decreasing in altitude
    if (motors->_air_density_ratio > 0.3f && motors->_air_density_ratio < 1.5f) {
        ret *= 1.0f / math_constrain_float(motors->_air_density_ratio, 0.5f, 1.25f);
    }
#endif

    return ret;
}

// convert actuator output (0~1) range to pwm range
int16_t mb_motors_output_to_pwm(mb_motors_t motors, float actuator)
{
    mb_motors_mc_t motors_mc = (mb_motors_mc_t)motors;

    float pwm_output;
    if (motors->_spool_state == MB_MOTOR_SHUT_DOWN) {
        // in shutdown mode, use PWM 0 or minimum PWM
        if (motors_mc->_disarm_disable_pwm && !mb_motors_armed(motors)) {
            pwm_output = 0;
        } else {
            pwm_output = mb_motors_get_pwm_output_min(motors);
        }
    } else {
        // in all other spool modes, covert to desired PWM
        pwm_output = mb_motors_get_pwm_output_min(motors) + (mb_motors_get_pwm_output_max(motors) - mb_motors_get_pwm_output_min(motors)) * actuator;
    }

    return pwm_output;
}

// adds slew rate limiting to actuator output
void mb_motors_set_actuator_with_slew(mb_motors_t motors, float* actuator_output, float input)
{
    mb_motors_mc_t motors_mc = (mb_motors_mc_t)motors;

    /*
    If MOT_SLEW_UP_TIME is 0 (default), no slew limit is applied to increasing output.
    If MOT_SLEW_DN_TIME is 0 (default), no slew limit is applied to decreasing output.
    MOT_SLEW_UP_TIME and MOT_SLEW_DN_TIME are constrained to 0.0~0.5 for sanity.
    If spool mode is shutdown, no slew limit is applied to allow immediate disarming of motors.
    */

    // Output limits with no slew time applied
    float output_slew_limit_up = 1.0f;
    float output_slew_limit_dn = 0.0f;

    // If MOT_SLEW_UP_TIME is set, calculate the highest allowed new output value, constrained 0.0~1.0
    if (math_flt_positive(motors_mc->_slew_up_time)) {
        float output_delta_up_max = motors->_dt / (math_constrain_float(motors_mc->_slew_up_time, 0.0f, 0.5f));
        output_slew_limit_up = math_constrain_float(*actuator_output + output_delta_up_max, 0.0f, 1.0f);
    }

    // If MOT_SLEW_DN_TIME is set, calculate the lowest allowed new output value, constrained 0.0~1.0
    if (math_flt_positive(motors_mc->_slew_dn_time)) {
        float output_delta_dn_max = motors->_dt / (math_constrain_float(motors_mc->_slew_dn_time, 0.0f, 0.5f));
        output_slew_limit_dn = math_constrain_float(*actuator_output - output_delta_dn_max, 0.0f, 1.0f);
    }

    // Constrain change in output to within the above limits
    *actuator_output = math_constrain_float(input, output_slew_limit_dn, output_slew_limit_up);
}

// gradually increase actuator output to spin_min
float mb_motors_actuator_spin_up_to_ground_idle(mb_motors_const_t motors)
{
    return math_constrain_float(((mb_motors_mc_t)motors)->_spin_up_ratio, 0.0f, 1.0f) * ((mb_motors_mc_t)motors)->_spin_min;
}

void mb_motors_bind_motors_function(mb_motors_ops_t ops_frontend, mb_motors_ops_t ops_multicopter)
{
    if (ops_frontend == ops_multicopter) {
        return;
    }

    if (ops_frontend->arming_checks == NULL && ops_multicopter->arming_checks != NULL) {
        ops_frontend->arming_checks = ops_multicopter->arming_checks;
    }

    if (ops_frontend->set_roll_pitch == NULL && ops_multicopter->set_roll_pitch != NULL) {
        ops_frontend->set_roll_pitch = ops_multicopter->set_roll_pitch;
    }

    if (ops_frontend->get_throttle_hover == NULL && ops_multicopter->get_throttle_hover != NULL) {
        ops_frontend->get_throttle_hover = ops_multicopter->get_throttle_hover;
    }

    if (ops_frontend->get_lost_motor == NULL && ops_multicopter->get_lost_motor != NULL) {
        ops_frontend->get_lost_motor = ops_multicopter->get_lost_motor;
    }

    if (ops_frontend->set_update_rate == NULL && ops_multicopter->set_update_rate != NULL) {
        ops_frontend->set_update_rate = ops_multicopter->set_update_rate;
    }

    if (ops_frontend->init == NULL && ops_multicopter->init != NULL) {
        ops_frontend->init = ops_multicopter->init;
    }

    if (ops_frontend->set_frame_class_and_type == NULL && ops_multicopter->set_frame_class_and_type != NULL) {
        ops_frontend->set_frame_class_and_type = ops_multicopter->set_frame_class_and_type;
    }

    if (ops_frontend->output == NULL && ops_multicopter->output != NULL) {
        ops_frontend->output = ops_multicopter->output;
    }

    if (ops_frontend->output_min == NULL && ops_multicopter->output_min != NULL) {
        ops_frontend->output_min = ops_multicopter->output_min;
    }

    if (ops_frontend->get_motor_mask == NULL && ops_multicopter->get_motor_mask != NULL) {
        ops_frontend->get_motor_mask = ops_multicopter->get_motor_mask;
    }

    if (ops_frontend->get_roll_factor == NULL && ops_multicopter->get_roll_factor != NULL) {
        ops_frontend->get_roll_factor = ops_multicopter->get_roll_factor;
    }

    if (ops_frontend->get_pitch_factor == NULL && ops_multicopter->get_pitch_factor != NULL) {
        ops_frontend->get_pitch_factor = ops_multicopter->get_pitch_factor;
    }

    if (ops_frontend->is_motor_enabled == NULL && ops_multicopter->is_motor_enabled != NULL) {
        ops_frontend->is_motor_enabled = ops_multicopter->is_motor_enabled;
    }

    if (ops_frontend->init_targets_on_arming == NULL && ops_multicopter->init_targets_on_arming != NULL) {
        ops_frontend->init_targets_on_arming = ops_multicopter->init_targets_on_arming;
    }

    if (ops_frontend->rc_write == NULL && ops_multicopter->rc_write != NULL) {
        ops_frontend->rc_write = ops_multicopter->rc_write;
    }

    if (ops_frontend->Log_Write == NULL && ops_multicopter->Log_Write != NULL) {
        ops_frontend->Log_Write = ops_multicopter->Log_Write;
    }

    if (ops_frontend->output_armed_stabilizing == NULL && ops_multicopter->output_armed_stabilizing != NULL) {
        ops_frontend->output_armed_stabilizing = ops_multicopter->output_armed_stabilizing;
    }

    if (ops_frontend->rc_write_angle == NULL && ops_multicopter->rc_write_angle != NULL) {
        ops_frontend->rc_write_angle = ops_multicopter->rc_write_angle;
    }

    if (ops_frontend->rc_set_freq == NULL && ops_multicopter->rc_set_freq != NULL) {
        ops_frontend->rc_set_freq = ops_multicopter->rc_set_freq;
    }

    if (ops_frontend->update_throttle_filter == NULL && ops_multicopter->update_throttle_filter != NULL) {
        ops_frontend->update_throttle_filter = ops_multicopter->update_throttle_filter;
    }

    if (ops_frontend->save_params_on_disarm == NULL && ops_multicopter->save_params_on_disarm != NULL) {
        ops_frontend->save_params_on_disarm = ops_multicopter->save_params_on_disarm;
    }

    if (ops_frontend->_get_frame_string == NULL && ops_multicopter->_get_frame_string != NULL) {
        ops_frontend->_get_frame_string = ops_multicopter->_get_frame_string;
    }

    if (ops_frontend->get_type_string == NULL && ops_multicopter->get_type_string != NULL) {
        ops_frontend->get_type_string = ops_multicopter->get_type_string;
    }

    if (ops_frontend->_output_test_seq == NULL && ops_multicopter->_output_test_seq != NULL) {
        ops_frontend->_output_test_seq = ops_multicopter->_output_test_seq;
    }

    if (ops_frontend->assign_param == NULL && ops_multicopter->assign_param != NULL) {
        ops_frontend->assign_param = ops_multicopter->assign_param;
    }
}

/// 

// output a thrust to all motors that match a given motor
// mask. This is used to control tiltrotor motors in forward
// flight. Thrust is in the range 0 to 1
void mb_motors_OutputMotorMask(mb_motors_t motors, float thrust, uint16_t mask, float rudder_dt)
{
    mb_motors_mc_t motors_mc = (mb_motors_mc_t)motors;

    const int16_t pwm_min = mb_motors_get_pwm_output_min(motors);
    const int16_t pwm_range = mb_motors_get_pwm_output_max(motors) - pwm_min;

    for (uint8_t i = 0; i < MB_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motors_mc->motor_enabled[i]) {
            if ((mask & (1U << i)) && mb_motors_armed(motors) && mb_motors_get_interlock(motors)) {
                /*
                 apply rudder mixing differential thrust
                 copter frame roll is plane frame yaw as this only
                 apples to either tilted motors or tailsitters
                 */
                float diff_thrust = mb_motors_get_roll_factor(motors, i) * rudder_dt * 0.5f;
                mb_motors_set_actuator_with_slew(motors, &motors_mc->_actuator[i], thrust + diff_thrust);
                int16_t pwm_output = pwm_min + pwm_range * motors_mc->_actuator[i];
                mb_motors_rc_write(motors, i, pwm_output);
            } else {
                mb_motors_rc_write(motors, i, pwm_min);
            }
        }
    }
}

// return current_limit as a number from 0 ~ 1 in the range throttle_min to throttle_max
float mb_motors_GetCurrentLimitMaxThrottle(mb_motors_t motors)
{
    mb_motors_mc_t motors_mc = (mb_motors_mc_t)motors;

    float _batt_current;

    if (motors_mc->_batt_current_max <= 0 || // return maximum if current limiting is disabled
        !mb_motors_armed(motors) || // remove throttle limit if disarmed
        !battmonitor_current_amps(&_batt_current, motors_mc->_batt_idx)) { // no current monitoring is available
        motors_mc->_throttle_limit = 1.0f;
        return 1.0f;
    }

    float _batt_resistance = battmonitor_get_resistance(motors_mc->_batt_idx);

    if (math_flt_zero(_batt_resistance)) {
        motors_mc->_throttle_limit = 1.0f;
        return 1.0f;
    }

    // calculate the maximum current to prevent voltage sag below _batt_voltage_min
    float batt_current_max = MIN(motors_mc->_batt_current_max, _batt_current + (battmonitor_voltage(motors_mc->_batt_idx) - motors_mc->_batt_voltage_min) / _batt_resistance);

    float batt_current_ratio = _batt_current / batt_current_max;

    motors_mc->_throttle_limit += (motors->_dt / (motors->_dt + motors_mc->_batt_current_time_constant)) * (1.0f - batt_current_ratio);

    // throttle limit drops to 20% between hover and full throttle
    motors_mc->_throttle_limit = math_constrain_float(motors_mc->_throttle_limit, 0.2f, 1.0f);

    // limit max throttle
    return mb_motors_get_throttle_hover(motors) + ((1.0 - mb_motors_get_throttle_hover(motors)) * motors_mc->_throttle_limit);
}

// output booster throttle, if any
void mb_motors_OutputBoostThrottle(mb_motors_t motors)
{
    mb_motors_mc_t motors_mc = (mb_motors_mc_t)motors;

    if (motors_mc->_boost_scale > 0) {
        float throttle = math_constrain_float(mb_motors_get_throttle(motors) * motors_mc->_boost_scale, 0.0f, 1.0f);
        srv_channels_set_output_scaled(k_boost_throttle, throttle * 1000);
    } else {
        srv_channels_set_output_scaled(k_boost_throttle, 0);
    }
}

// output roll/pitch/yaw/thrust
void mb_motors_OutputRpyt(mb_motors_t motors)
{
    srv_channels_set_output_scaled(k_roll_out, motors->_roll_in_ff * 4500);
    srv_channels_set_output_scaled(k_pitch_out, motors->_pitch_in_ff * 4500);
    srv_channels_set_output_scaled(k_yaw_out, motors->_yaw_in_ff * 4500);
    srv_channels_set_output_scaled(k_thrust_out, mb_motors_get_throttle(motors) * 1000);
}

// get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
//  this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
uint32_t mb_motors_GetMotorMask(mb_motors_t motors)
{
    (void)motors;
    return srv_channels_get_output_channel_mask(k_boost_throttle);
}

/// 
// output - sends commands to the motors
static void output(mb_motors_t motors)
{
    // update throttle filter
    mb_motors_update_throttle_filter(motors);

    // calc filtered battery voltage and lift_max
    mb_motors_update_lift_max_from_batt_voltage(motors);

    // run spool logic
    mb_motors_output_logic(motors);

    // calculate thrust
    mb_motors_output_armed_stabilizing(motors);

    // apply any thrust compensation for the frame
    mb_motors_thrust_compensation(motors);

    // convert rpy_thrust values to pwm
    mb_motors_output_to_motors(motors);

    // output any booster throttle
    mb_motors_output_boost_throttle(motors);

    // output raw roll/pitch/yaw/thrust
    mb_motors_output_rpyt(motors);
};

// sends minimum values out to the motors
static void output_min(mb_motors_t motors)
{
    mb_motors_set_desired_spool_state(motors, MB_MOTOR_DESIRED_SHUT_DOWN);
    motors->_spool_state = MB_MOTOR_SHUT_DOWN;
    mb_motors_output(motors);
}

static float get_throttle_hover(mb_motors_const_t motors)
{
    mb_motors_mc_const_t motors_mc = (mb_motors_mc_const_t)motors;

    return math_constrain_float(motors_mc->_throttle_hover, MB_MOTORS_THST_HOVER_MIN, MB_MOTORS_THST_HOVER_MAX);
}

// return whether a motor is enabled or not
static bool is_motor_enabled(mb_motors_t motors, uint8_t i)
{
    return ((mb_motors_mc_t)motors)->motor_enabled[i];
}

// 10hz logging of voltage scaling and max trust
static void Log_Write(mb_motors_t motors)
{

}

// Run arming checks
static bool arming_checks(mb_motors_const_t motors, size_t buflen, char *buffer)
{
    mb_motors_mc_const_t motors_mc = (mb_motors_mc_const_t)motors;

    // run base class checks
    if (!mb_motors_ArmingChecks(motors, buflen, buffer)) {
        return false;
    }

    // Check output function is setup for each motor
    for (uint8_t i = 0; i < MB_MOTORS_MAX_NUM_MOTORS; i++) {
        if (!motors_mc->motor_enabled[i]) {
            continue;
        }
        uint8_t chan;
        Aux_servo_function_t function = srv_channels_get_motor_function(i);
        if (!srv_channels_find_channel(function, &chan)) {
            rt_snprintf(buffer, buflen, "no SERVOx_FUNCTION set to Motor%u", i + 1);
            return false;
        }
    }

    // Check param config
    if (motors_mc->_spin_min > 0.3f) {
        rt_snprintf(buffer, buflen, "%sSPIN_MIN too high %.2f > 0.3", MB_MOTORS_PARAM_PREFIX, motors_mc->_spin_min);
        return false;
    }
    if (motors_mc->_spin_arm > motors_mc->_spin_min) {
        rt_snprintf(buffer, buflen, "%sSPIN_ARM > %sSPIN_MIN", MB_MOTORS_PARAM_PREFIX, MB_MOTORS_PARAM_PREFIX);
        return false;
    }
    if (!mb_motors_check_mot_pwm_params(motors)) {
        rt_snprintf(buffer, buflen, "Check %sPWM_MIN and %sPWM_MAX", MB_MOTORS_PARAM_PREFIX, MB_MOTORS_PARAM_PREFIX);
        return false;
    }

    return true;
}

// update the throttle input filter
static void update_throttle_filter(mb_motors_t motors)
{
    mb_motors_mc_const_t motors_mc = (mb_motors_mc_const_t)motors;

    const float last_thr = lpf_get_output(&motors->_throttle_filter);

    if (mb_motors_armed(motors)) {
        lpf_apply2(&motors->_throttle_filter, motors->_throttle_in, motors->_dt);
        // constrain filtered throttle
        if (lpf_get_output(&motors->_throttle_filter) < 0.0f) {
            lpf_reset(&motors->_throttle_filter, 0.0f);
        }
        if (lpf_get_output(&motors->_throttle_filter) > 1.0f) {
            lpf_reset(&motors->_throttle_filter, 1.0f);
        }
    } else {
        lpf_reset(&motors->_throttle_filter, 0.0f);
    }

    float new_thr = lpf_get_output(&motors->_throttle_filter);

    if (!math_flt_equal(last_thr, new_thr)) {
        derivative_filter7_update(&motors->_throttle_slew, new_thr, time_micros());
    }

    // calculate slope normalized from per-micro
    const float rate = fabsf(derivative_filter7_slope(&motors->_throttle_slew) * 1000000);
    motors->_throttle_slew_rate = lpf_apply2(&motors->_throttle_slew_filter, rate, motors->_dt);
}

// save parameters as part of disarming
static void save_params_on_disarm(mb_motors_t motors)
{
    mb_motors_mc_t motors_mc = (mb_motors_mc_t)motors;

    // save hover throttle
    if (motors_mc->_throttle_hover_learn == MB_MOTOR_HOVER_LEARN_AND_SAVE) {
        param_save_obj_by_variable2("MOTORS", &motors_mc->_throttle_hover);
    }
}

// set the value of motor module parameters
static void assign_param(mb_motors_t motors)
{
    mb_motors_mc_t motors_mc = (mb_motors_mc_t)motors;

    param_link_variable(PARAM_ID(MOTORS,MOT_YAW_HEADROOM), &motors_mc->_yaw_headroom);
    param_link_variable(PARAM_ID(MOTORS,MOT_THST_EXPO), &motors_mc->_thrust_curve_expo);
    param_link_variable(PARAM_ID(MOTORS,MOT_SLEW_UP_TIME), &motors_mc->_slew_up_time);
    param_link_variable(PARAM_ID(MOTORS,MOT_SLEW_DN_TIME), &motors_mc->_slew_dn_time);
    param_link_variable(PARAM_ID(MOTORS,MOT_SAFE_TIME), &motors_mc->_safe_time);
    param_link_variable(PARAM_ID(MOTORS,MOT_SPIN_MIN), &motors_mc->_spin_min);
    param_link_variable(PARAM_ID(MOTORS,MOT_SPIN_MAX), &motors_mc->_spin_max);
    param_link_variable(PARAM_ID(MOTORS,MOT_SPIN_ARM), &motors_mc->_spin_arm);
    param_link_variable(PARAM_ID(MOTORS,MOT_BAT_VOLT_MAX), &motors_mc->_batt_voltage_max);
    param_link_variable(PARAM_ID(MOTORS,MOT_BAT_VOLT_MIN), &motors_mc->_batt_voltage_min);
    param_link_variable(PARAM_ID(MOTORS,MOT_BAT_CURR_MAX), &motors_mc->_batt_current_max);
    param_link_variable(PARAM_ID(MOTORS,MOT_BAT_CURR_TC), &motors_mc->_batt_current_time_constant);
    param_link_variable(PARAM_ID(MOTORS,MOT_BAT_IDX), &motors_mc->_batt_idx);
    param_link_variable(PARAM_ID(MOTORS,MOT_PWM_TYPE), &motors->_pwm_type);
    param_link_variable(PARAM_ID(MOTORS,MOT_PWM_MIN), &motors_mc->_pwm_min);
    param_link_variable(PARAM_ID(MOTORS,MOT_PWM_MAX), &motors_mc->_pwm_max);
    param_link_variable(PARAM_ID(MOTORS,MOT_THST_HOVER), &motors_mc->_throttle_hover);
    param_link_variable(PARAM_ID(MOTORS,MOT_HOVER_LEARN), &motors_mc->_throttle_hover_learn);
    param_link_variable(PARAM_ID(MOTORS,MOT_SAFE_DISARM), &motors_mc->_disarm_disable_pwm);
    param_link_variable(PARAM_ID(MOTORS,MOT_YAW_SV_ANGLE), &motors_mc->_yaw_servo_angle_max_deg);
    param_link_variable(PARAM_ID(MOTORS,MOT_SPOOL_TIME), &motors_mc->_spool_up_time);
    param_link_variable(PARAM_ID(MOTORS,MOT_BOOST_SCALE), &motors_mc->_boost_scale);
}
/*------------------------------------test------------------------------------*/


